﻿#include "pickarm.h"

PickArm::PickArm(TrayConfig *sensorTrayConfig, TrayConfig *lensTrayConfig,TrayConfig *rejectTrayConfig,
                 SensorTrayMap *sensorTrayMap,   LensTrayMap *lensTrayMap, SensorTrayMap *rejectTrayMap): Worker(WorkerName::PickArm),
        sensorTrayConfig(sensorTrayConfig), lensTrayConfig(lensTrayConfig),rejectTrayConfig(rejectTrayConfig),
        sensorTrayMap(sensorTrayMap), lensTrayMap(lensTrayMap), rejectTrayMap(rejectTrayMap)
{
    paConfig = new PickArmConfig();
    paConfigFile
        = new ConfigFile("PickArmConfig", paConfig, QString("%1/PickArmConfig.json").arg(getWorkerConfigDir()));
    paConfigFile->populate();
}

PickArm::~PickArm() {}

void PickArm::initEventHandle()
{
    PickerZ1 = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::PickerZ1);
    PickerZ2 = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::PickerZ2);
    PickerT1 = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::PickerT1);
    PickerT2 = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::PickerT2);

    trayLoaderX = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::TrayLoaderX);
    pickerArmY = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::PickArmY);

    pickerZ1Motor = MotionElementContainer::getIns()->getItem<SCAxis>(AxisName::PickerZ1);
    pickerZ2Motor = MotionElementContainer::getIns()->getItem<SCAxis>(AxisName::PickerZ2);

    PickerXYmodule = MotionElementContainer::getIns()->getItem<XYModule>(XYModuleName::PickArmXY);
    SUTxyz = MotionElementContainer::getIns()->getItem<XYZModule>(XYZModuleName::SUTXYZ);

    pickerCaliGlassLocation = VisionManager::getIns().getVisionLocation(VLName::PickerCaliGlassLocation);
    sensorTrayDownLookLocation = VisionManager::getIns().getVisionLocation(VLName::SensorTrayDownLookLocation);
    lensTrayDownLookLocation = VisionManager::getIns().getVisionLocation(VLName::LensTrayDownLookLocation);
    rejectTrayDownLookLocation = VisionManager::getIns().getVisionLocation(VLName::RejectTrayDownLookLocation);

    lensPickerVacuum = MotionElementContainer::getIns()->getItem<SCVacuum>(VacuumName::LPAVacuum);
    sensorPickerVacuum = MotionElementContainer::getIns()->getItem<SCVacuum>(VacuumName::SPAVacuum);
    ngSensorPickerVacuum = MotionElementContainer::getIns()->getItem<SCVacuum>(VacuumName::SPARejectVac);
    sutVacuum = MotionElementContainer::getIns()->getItem<SCVacuum>(VacuumName::SUTVacuum);
    lutVacuum = MotionElementContainer::getIns()->getItem<SCVacuum>(VacuumName::LUTVacuum);
    RejectLVCM = MotionElementContainer::getIns()->getItem<SCVacuum>(VacuumName::LUTRejectVac);

    ngSensorPickerCylinder = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::NgSensorPickerCyl);
    sutCylinder = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::SutClamp);

}

void PickArm::startEventHandle(const WorkerEvent &event)
{
    Q_UNUSED(event)
    isPaStopped = false;
    pushEvent(WorkerEvent("stratAuto"));
}

void PickArm::allWorkerStopped()
{
    isPaStopped = true;
}

void PickArm::purgeEventHandle()
{
    purgeEventHandled();
}

void PickArm::resetEventHandle()
{
    isPaStopped = false;
}

void PickArm::homeWorker(bool moveToReadyPosAfterHome)
{
    ngSensorPickerCylinder->set(false, false);
    PickerZ1->axis()->home(false);
    PickerZ2->axis()->home(false);

    PickerT1->axis()->home(false);
    PickerT2->axis()->home(false);

    ngSensorPickerCylinder->wait(false);
    PickerZ1->axis()->waitHomeDone();
    PickerZ2->axis()->waitHomeDone();

    PickerXYmodule->xAxis()->home(false);
    PickerXYmodule->yAxis()->home(false);

    PickerXYmodule->xAxis()->waitHomeDone();
    PickerXYmodule->yAxis()->waitHomeDone();
    PickerT1->axis()->waitHomeDone();
    PickerT2->axis()->waitHomeDone();

    if (moveToReadyPosAfterHome)
    {
        moveToStanbyPos();
    }
}

//
void PickArm::moveToStanbyPos()
{
    PickerZ1->moveTo(SAxisPosName::StandbyPos, false);
    PickerZ2->moveTo(SAxisPosName::StandbyPos,false);
    PickerT1->moveTo(SAxisPosName::StandbyPos, false);
    PickerT2->moveTo(SAxisPosName::StandbyPos, false);
    ngSensorPickerCylinder->set(false);

    PickerZ1->waitArrivedPos(SAxisPosName::StandbyPos);
    PickerZ2->waitArrivedPos(SAxisPosName::StandbyPos);
    PickerT1->waitArrivedPos(SAxisPosName::StandbyPos);
    PickerT2->waitArrivedPos(SAxisPosName::StandbyPos);

    PickerXYmodule->moveTo(XYPosName::StandbyPos);
}

void PickArm::moveUpAllZ()
{
    try
    {
        PickerZ1->moveTo(SAxisPosName::StandbyPos, false);
        PickerZ2->moveTo(SAxisPosName::StandbyPos);
        ngSensorPickerCylinder->set(false);
        PickerZ1->waitArrivedPos(SAxisPosName::StandbyPos);
    }
    catch (SilicoolException &se)
    {
        ngSensorPickerCylinder->set(false);
        if (UIOperation::getIns()->okCancelConfirm(tr("Picke3Z cylinder action up faile, do you want to retry?")))
        {
            ngSensorPickerCylinder->set(false);
        }
        throw se;
    }
}

PrOffset PickArm::getCaliGlassPrOffset()
{
    PrOffset prOffset;
    if (!pickerCaliGlassLocation->performPR(prOffset))
    {
        throw SilicolAbort(tr("pickerCaliGlassLocation perform pr failed!"));
    }
    return prOffset;
}

void PickArm::moveToPickCalibrateGlassPos(bool isSensorPicker)
{
    if (isSensorPicker)
    {
        PickerXYmodule->moveTo(XYPosName::Picker1GetcalibGrassPos);
    }
    else
    {
        PickerXYmodule->moveTo(XYPosName::Picker2GetcalibGrassPos);
    }
}

void PickArm::pickMaterial(bool isSensorPicker)
{
    moveUpAllZ();
    try
    {
        if (isSensorPicker)
        {
            PickerZ1->moveTo(SAxisPosName::LoadMaterialPos);

            if(!paConfig->isDryRun())
            {
                sensorPickerVacuum->set(true);
                if (!sensorPickerVacuum->waitUntilTimeout(true))
                {
                    pickMaterialFailedHandle(isSensorPicker);
                    return;
                }
                if (paConfig->delayOfterOpenVacuum() > 0)
                {
                    QThread::msleep(paConfig->delayOfterOpenVacuum());
                }
            }

            PickerZ1->moveTo(SAxisPosName::StandbyPos);
        }
        else
        {
            PickerZ2->moveTo(SAxisPosName::LoadMaterialPos);

            if(!paConfig->isDryRun())
            {
                lensPickerVacuum->set(true);
                if (!lensPickerVacuum->waitUntilTimeout(true))
                {
                    lensPickerVacuum->set(false);

                }
                if (paConfig->delayOfterOpenVacuum() > 0)
                {
                    QThread::msleep(paConfig->delayOfterOpenVacuum());
                }
            }
            PickerZ2->moveTo(SAxisPosName::StandbyPos);
        }
    }
    catch (SilicoolException &se)
    {
        moveUpAllZ();
        throw se;
    }
}

void PickArm::pickCalibGlass(bool isSensorPicker)
{
    moveUpAllZ();
    try
    {
        if (isSensorPicker)
        {
            PickerZ1->moveTo(SAxisPosName::PickCalibraGrassPos);

            if(!paConfig->isDryRun())
            {
                sensorPickerVacuum->set(true);
                if (!sensorPickerVacuum->waitUntilTimeout(true))
                {
                    sensorPickerVacuum->set(false);
                    throw ActionError("Lens Picker", tr("Pick Sensor failed! Vacuum did not detect feedback signal!"));
                }
                if (paConfig->delayOfterOpenVacuum() > 0)
                {
                    QThread::msleep(paConfig->delayOfterOpenVacuum());
                }
            }

            PickerZ1->moveTo(SAxisPosName::StandbyPos);
        }
        else
        {
            PickerZ2->moveTo(SAxisPosName::PickCalibraGrassPos);

            if(!paConfig->isDryRun())
            {
                lensPickerVacuum->set(true);
                if (!lensPickerVacuum->waitUntilTimeout(true))
                {
                    lensPickerVacuum->set(false);
                    throw ActionError("Sensor Picker", tr("Pick lens failed! Vacuum did not detect feedback signal!"));
                }
                if (paConfig->delayOfterOpenVacuum() > 0)
                {
                    QThread::msleep(paConfig->delayOfterOpenVacuum());
                }
            }
            PickerZ2->moveTo(SAxisPosName::StandbyPos);
        }
    }
    catch (SilicoolException &se)
    {
        moveUpAllZ();
        throw se;
    }
}

void PickArm::placeCalibGlass(bool isSensorPicker)
{
    moveUpAllZ();
    try
    {
        if (isSensorPicker)
        {
            PickerZ1->moveTo(SAxisPosName::PickCalibraGrassPos);

            if(!paConfig->isDryRun())
            {
                sensorPickerVacuum->set(false);

                if (paConfig->delayOfterOpenVacuum() > 0)
                {
                    QThread::msleep(paConfig->delayOfterOpenVacuum());
                }
            }

            PickerZ1->moveTo(SAxisPosName::StandbyPos);
        }
        else
        {
            PickerZ2->moveTo(SAxisPosName::PickCalibraGrassPos);

            if(!paConfig->isDryRun())
            {
                lensPickerVacuum->set(false);
                if (paConfig->delayOfterOpenVacuum() > 0)
                {
                    QThread::msleep(paConfig->delayOfterOpenVacuum());
                }
            }
            PickerZ2->moveTo(SAxisPosName::StandbyPos);
        }
    }
    catch (SilicoolException &se)
    {
        moveUpAllZ();
        throw se;
    }
}

void PickArm::placeBackMaterial(bool isSenserPicker)
{
    moveUpAllZ();
    try
    {
        if (isSenserPicker)
        {
            PickerZ1->moveTo(SAxisPosName::LoadMaterialPos);

            if(!paConfig->isDryRun())
            {
                sensorPickerVacuum->set(false);

                if (paConfig->delayOfterOpenVacuum() > 0)
                {
                    QThread::msleep(paConfig->delayOfterOpenVacuum());
                }
            }

            PickerZ1->moveTo(SAxisPosName::StandbyPos);
        }
        else
        {
            PickerZ2->moveTo(SAxisPosName::LoadMaterialPos);

            if(!paConfig->isDryRun())
            {
                lensPickerVacuum->set(false);
                if (paConfig->delayOfterOpenVacuum() > 0)
                {
                    QThread::msleep(paConfig->delayOfterOpenVacuum());
                }
            }
            PickerZ2->moveTo(SAxisPosName::StandbyPos);
        }
    }
    catch (SilicoolException &se)
    {
        moveUpAllZ();
        throw se;
    }
}

void PickArm::moveToPADownlookPrPos(bool isSensorPicker)
{
    if (isSensorPicker)
    {
        PickerXYmodule->moveTo(XYPosName::Picker1DownlookPrPos);
    }
    else
    {
        PickerXYmodule->moveTo(XYPosName::Picker2DownlookPrPos);
    }
}

PrOffset PickArm::getPickerCaliGlassPrOffset(bool isSensorPicker)
{
    PrOffset prOffset;
    bool res = false;

    if (!qFuzzyCompare(paConfig->picker1ToCameraOffsetY(), 0))
    {
        if (isSensorPicker)
        {
            pickerArmY->axis()->relMove(paConfig->picker1ToCameraOffsetY());
            res = sensorTrayDownLookLocation->performPR(prOffset);
        }
        else
        {
            pickerArmY->axis()->relMove(paConfig->picker2ToCameraOffsetY());
            res = lensTrayDownLookLocation->performPR(prOffset);
        }
    }

    if (!qFuzzyCompare(paConfig->picker1ToCameraOffsetY(), 0) || !qFuzzyCompare(paConfig->picker1ToCameraOffsetY(), 0))
    {
        if (isSensorPicker)
        {
            pickerArmY->axis()->relMove(paConfig->picker1ToCameraOffsetY() * -1);
        }
        else
        {
            pickerArmY->axis()->relMove(paConfig->picker2ToCameraOffsetY() * -1);
        }
    }

    if (!res)
    {
        throw SilicolAbort(tr("pickerCaliGlassLocation perform pr failed!"));
    }
    if (isSensorPicker)
    {
        prOffset.Y -= paConfig->picker1ToCameraOffsetY();
        prOffset.O_Y -= paConfig->picker1ToCameraOffsetY();
    }
    else
    {
        prOffset.Y -= paConfig->picker2ToCameraOffsetY();
        prOffset.O_Y -= paConfig->picker2ToCameraOffsetY();
    }

    return prOffset;
}

PrOffset PickArm::getTrayUpLookCameraOffset(bool isSensorTray)
{
    PrOffset prOffset;
    if (isSensorTray)
    {
        if (!sensorTrayDownLookLocation->performPR(prOffset))
        {
            throw SilicolAbort(tr("Sensor TrayUpLookCameraOffset perform pr failed!"));
        }
    }
    else
    {
        if (!lensTrayDownLookLocation->performPR(prOffset))
        {
            throw SilicolAbort(tr("Lens TrayUpLookCameraOffset perform pr failed!"));
        }
    }
    return prOffset;
}

PrOffset PickArm::getRejectTrayUpLookCameraOffset()
{
    PrOffset prOffset;

    if (!rejectTrayDownLookLocation->performPR(prOffset))
    {
        throw SilicolAbort(tr("Reject TrayUpLookCameraOffset perform pr failed!"));
    }

    return prOffset;
}

bool PickArm::pickeLens(int rowIndex, int columnIndex, int unitRowIndex, int unitColumnIndex)
{
    try {
            if(!paConfig->isDryRun())
            {
                if (lensPickerVacuum->get())
                {
                    NuzzleHasMaterial err;
                    auto option = ErrorHandler::getErrorHandlingOption(err);

                    if(option == ErrorHandlingOption::Continual_)
                    {

                    }
                    else {
                        throw SilicolAbort(tr("picker2 nuzzle has material, can not pick lens "));
                    }
                }
            }

            moveUpAllZ();

            auto lensPrPos = PickerXYmodule->getPos<XYModulePos>(XYPosName::LensTrayDownLookPos);
            double xPos = lensPrPos->xPos() - (unitColumnIndex -1) * lensTrayConfig->unitColumnDelta() - (columnIndex - 1) * lensTrayConfig->columnDelta();
            double yPos = lensPrPos->yPos() + (unitRowIndex -1) * lensTrayConfig->unitRowDelta() + ( rowIndex- 1) * lensTrayConfig->rowDelta();
            PickerXYmodule->moveTo(QPointF(xPos, yPos));

            PrOffset lensOffset = PrOffset(0, 0, 0);
            if(!paConfig->isDryRun())
            {
                if(!lensTrayDownLookLocation->performPR(lensOffset))
                {
                    MaterialPrErr err;
                    auto option = ErrorHandler::getErrorHandlingOption(err);
                    if(option == Retry_)
                    {
                        QVariantList arg;
                        arg << rowIndex << columnIndex << unitRowIndex << unitColumnIndex;
                        pushEvent(WorkerEvent("pickeLens", arg, WorkerEventPriority::ErrorHandle));
                        return false;
                    }
                    else if (option == ErrorHandlingOption::Next_) {
                        lensTrayMoveNext();
                        pushEvent(WorkerEvent("pickLensFunction", QVariantList(), WorkerEventPriority::ErrorHandle));
                        return false;
                    }
                    else {
                        return false;
                    }
                }
            }

            PickerXYmodule->moveTo(QPointF(xPos - lensOffset.X + paConfig->picker2ToCameraOffsetX(),
                                           yPos - lensOffset.Y + paConfig->picker2ToCameraOffsetY()));
            pickMaterial(false);
    } catch (SilicoolException &se) {
        return false;
    }

}

bool PickArm::pickSensor(int rowIndex, int columnIndex, int unitRowIndex, int unitColumnIndex)
{
    try {
        if(!paConfig->isDryRun())
        {
            if (sensorPickerVacuum->get())
            {
                NuzzleHasMaterial err;
                auto option = ErrorHandler::getErrorHandlingOption(err);

                if(option == ErrorHandlingOption::Continual_)
                {

                }
                else {
                    throw SilicolAbort(tr("picker1 nuzzle has material, can not pick sensor! "));
                }
            }
        }

        moveUpAllZ();

        auto senserPrPos = PickerXYmodule->getPos<XYModulePos>(XYPosName::SensorTrayDownLookPos);
        double xPos = senserPrPos->xPos() - (unitColumnIndex - 1) * sensorTrayConfig->unitColumnDelta() - (columnIndex - 1) * sensorTrayConfig->columnDelta();
        double yPos = senserPrPos->yPos() + (unitRowIndex -1) * sensorTrayConfig->unitRowDelta() + (rowIndex - 1) * sensorTrayConfig->rowDelta();
        PickerXYmodule->moveTo(QPointF(xPos, yPos));

        PrOffset senserPrOffset = PrOffset(0, 0, 0);

        if(!paConfig->isDryRun())
        {
            if(!sensorTrayDownLookLocation->performPR(senserPrOffset))
            {
                MaterialPrErr err;
                auto option = ErrorHandler::getErrorHandlingOption(err);
                if(option == Retry_)
                {
                    QVariantList arg;
                    arg << rowIndex << columnIndex << unitRowIndex << unitColumnIndex;
                    pushEvent(WorkerEvent("pickSensor", arg, WorkerEventPriority::ErrorHandle));
                    return false;
                }
                else if (option == ErrorHandlingOption::Next_) {
                    sensorTrayMoveNext();
                    pushEvent(WorkerEvent("pickSenorFunction", QVariantList(), WorkerEventPriority::ErrorHandle));
                    return false;
                }
                else {
                    return false;
                }
            }
        }

        PickerXYmodule->moveTo(QPointF(xPos - senserPrOffset.X +  paConfig->picker1ToCameraOffsetX() + paConfig->pickSensorOffsetX(),
                                      yPos - senserPrOffset.Y + paConfig->picker1ToCameraOffsetY() + paConfig->pickSensorOffsetY()));
        pickMaterial(true);
        return true;
    } catch (SilicoolException &se) {
        return false;
    }

}

void PickArm::placeOkProduct(int rowIndex, int columnIndex, int unitRowIndex, int unitColumnIndex)
{
    try
    {
        if(!paConfig->isDryRun())
        {
            if (!lensPickerVacuum->get())
            {
                lensPickerVacuum->set(false);
                throw SilicoolException(tr("Picker1 missed Ok Product!"));
            }
        }
        moveUpAllZ();

        auto placePos = PickerXYmodule->getPos<XYModulePos>(XYPosName::PlaceOKProductPos);

        double xPos = placePos->xPos() - (unitColumnIndex -1) * sensorTrayConfig->unitColumnDelta() - (columnIndex -1) * sensorTrayConfig->columnDelta();
        double yPos = placePos->yPos() + (unitRowIndex  -1) * sensorTrayConfig->unitRowDelta() + (rowIndex -1) * sensorTrayConfig->rowDelta();

        PickerXYmodule->moveTo(QPointF(xPos, yPos));

        PickerZ2->moveTo(SAxisPosName::PlaceOKProductPos);
        if(!paConfig->isDryRun())
        {
            lensPickerVacuum->set(false);
        }
        if (paConfig->delayOfterCloseVacuum() > 0)
        {
            QThread::msleep(paConfig->delayOfterCloseVacuum());
        }

        moveUpAllZ();
    }
    catch (SilicoolException &se)
    {
        moveUpAllZ();
        throw se;
    }
}

void PickArm::placeNgLens(int rowIndex, int columnIndex, int unitRowIndex, int unitColumnIndex)
{
    try
    {
        if(!paConfig->isDryRun())
        {
            if (!lensPickerVacuum->get())
            {
                lensPickerVacuum->set(false);
                throw SilicoolException(tr("Picker1 missed NG lens!"));
            }
        }
        moveUpAllZ();

        auto placePos = PickerXYmodule->getPos<XYModulePos>(XYPosName::LoadLensPos);
        PickerXYmodule->moveTo(QPointF(placePos->xPos() - (unitColumnIndex -1) * lensTrayConfig->unitColumnDelta() - (columnIndex - 1) * lensTrayConfig->columnDelta(),
                                       placePos->yPos() + (unitRowIndex -1) * lensTrayConfig->unitRowDelta() + ( rowIndex- 1) * lensTrayConfig->rowDelta()));

        PickerZ2->moveTo(SAxisPosName::LoadMaterialPos);

        if(!paConfig->isDryRun())
        {
            lensPickerVacuum->set(false);
            if (paConfig->delayOfterCloseVacuum() > 0)
            {
                QThread::msleep(paConfig->delayOfterCloseVacuum());
            }
        }
        moveUpAllZ();
    }
    catch (SilicoolException &se)
    {
        moveUpAllZ();
        throw se;
    }
}

void PickArm::placeNgSensor(int rowIndex, int columnIndex, int unitRowIndex, int unitColumnIndex)
{
    try
    {
        if(!paConfig->isDryRun())
        {
            if (!ngSensorPickerVacuum->get())
            {
                ngSensorPickerVacuum->set(false);
                throw SilicoolException(tr("Picker3 missed NG sensor!"));
            }
        }
        moveUpAllZ();

        auto pos = PickerXYmodule->getPos<XYModulePos>(XYPosName::PlaceNgSenserPos);
        PickerXYmodule->moveTo(QPointF(pos->xPos() - (unitColumnIndex - 1)* rejectTrayConfig->unitColumnDelta() - (columnIndex - 1) * rejectTrayConfig->columnDelta(),
                                       pos->yPos() + (unitRowIndex - 1)* rejectTrayConfig->unitRowDelta() + (rowIndex - 1)* rejectTrayConfig->rowDelta()));
        if(!paConfig->isDryRun())
        {
            ngSensorPickerVacuum->set(false);
        }
        if (paConfig->delayOfterCloseVacuum() > 0)
        {
            QThread::msleep(paConfig->delayOfterCloseVacuum());
        }

        moveUpAllZ();
    }
    catch (SilicoolException &se)
    {
        moveUpAllZ();
    }
}

void PickArm::setLensState(QPoint indexs, LensTrayMaterialData::MaterialState state)
{
    auto data = lensTrayMap->getData<LensTrayMaterialData>(indexs.x(), indexs.y());
    data->setMaterialState(state);
}

void PickArm::setSensorState(QPoint indexs, SensorTrayMaterialData::MaterialState state)
{
    auto data = sensorTrayMap->getData<SensorTrayMaterialData>(indexs.x(), indexs.y());
    data->setMaterialState(state);
}

void PickArm::setRejectState(QPoint indexs, SensorTrayMaterialData::MaterialState state)
{
    auto data = rejectTrayMap->getData<SensorTrayMaterialData>(indexs.x(), indexs.y());
    data->setMaterialState(state);
}

void PickArm::lensTrayMoveNext()
{
    paConfig->selfAddPickLensRowIndexInside();

    if(paConfig->currentPickLensRowIndexInside() > lensTrayConfig->rowsInsideUnit())
    {
        paConfig->setCurrentPickLensRowIndexInside(1);
        paConfig->selfAddPickLensColumnIndexInside();
    }

    if(paConfig->currentPickLensColumnIndexInside() > lensTrayConfig->columnsInsideUnit())
    {
        paConfig->setCurrentPickLensColumnIndexInside(1);
        paConfig->selfAddPickLensUnitRowIndex();
    }

    if(paConfig->currentPickLensUnitRowIndex() > lensTrayConfig->unitRows())
    {
        paConfig->setCurrentPickLensUnitRowIndex(1);
        paConfig->selfAddPickLensUnitColumnIndex();
    }
}

void PickArm::sensorTrayMoveNext()
{
    paConfig->selfAddPickSenserRowIndexInside();

    if(paConfig->currentPickSenserRowIndexInside() > sensorTrayConfig->rowsInsideUnit())
    {
        paConfig->setCurrentPickSenserRowIndexInside(1);
        paConfig->selfAddPickSenserColumnIndexInside();
    }

    if(paConfig->currentPickSenserColumnIndexInside() > sensorTrayConfig->columnsInsideUnit())
    {
        paConfig->setCurrentPickSenserColumnIndexInside(1);
        paConfig->selfAddPickSenserUnitRowIndex();
    }

    if(paConfig->currentPickSenserUnitRowIndex() > sensorTrayConfig->unitRows())
    {
        paConfig->setCurrentPickSenserUnitRowIndex(1);
        paConfig->selfAddPickSenserUnitColumnIndex();
    }
}

void PickArm::placeProducttMoveNext()
{
    paConfig->selfAddPlaceProductRowIndexInside();

    if(paConfig->currentPlaceProductRowIndexInside() > sensorTrayConfig->rowsInsideUnit())
    {
        paConfig->setCurrentPlaceProductRowIndexInside(1);
        paConfig->selfAddPlaceProductColumnIndexInside();
    }

    if(paConfig->currentPlaceProductColumnIndexInside() > sensorTrayConfig->columnsInsideUnit())
    {
        paConfig->setCurrentPlaceProductColumnIndexInside(1);
        paConfig->selfAddPlaceProductUintRowIndex();
    }

    if(paConfig->currentPlaceProductUintRowIndex() > sensorTrayConfig->unitRows())
    {
        paConfig->setCurrentPlaceProductUintRowIndex(1);
        paConfig->selfAddPlaceProductUintColumnIndex();
    }
}

void PickArm::pickMaterialFailedHandle(bool isSensorPicker)
{
    moveUpAllZ();

    PickMaterialErr err;
    auto option = ErrorHandler::getErrorHandlingOption(err);

    if(option == Retry_)
    {
        QVariantList arg;
        arg<< isSensorPicker;
        pushEvent(WorkerEvent("pickMaterial", arg, WorkerEventPriority::ErrorHandle));
        return;
    }
    if(option == ErrorHandlingOption::Next_)
    {
        QVariantList arg;
        arg<< isSensorPicker;
        if(isSensorPicker)
        {
            sensorTrayMoveNext();
        }
        else {
            lensTrayMoveNext();
        }
        pushEvent(WorkerEvent("pickMaterial", arg, WorkerEventPriority::ErrorHandle));
        return;
    }

}

// auto
void PickArm::stratAuto()
{
    checkIsInit();
    QVariantList arg;
    arg<<AAResult::FirstLoad;
    pushEvent(WorkerEvent("loadMaterialFunction", arg));
}

void PickArm::loadMaterialFunction(QString type)
{
    checkIsInit();
    moveUpAllZ();

    if(type == AAResult::FirstLoad)
    {
        PickerXYmodule->moveTo(XYPosName::StandbyPos);
        if(finishPlanPlace)
        {
            return;
        }

        pushEvent(WorkerEvent("pickSenorFunction", QVariantList(), WorkerEventPriority::PAAuto));
        pushEvent(WorkerEvent("pickLensFunction", QVariantList(), WorkerEventPriority::PAAuto));
    }
    else if (type == AAResult::GoodProduct) {
        pushEvent(WorkerEvent("placeOkProductFunction", QVariantList(), WorkerEventPriority::PAAuto));

        if(!finishPlanPlace){
            pushEvent(WorkerEvent("pickSenorFunction", QVariantList(), WorkerEventPriority::PAAuto));
            pushEvent(WorkerEvent("pickLensFunction", QVariantList(), WorkerEventPriority::PAAuto));
        }
    }
    else if (type == AAResult::NgProduct) {
        pushEvent(WorkerEvent("placeNGProductFunction", QVariantList(), WorkerEventPriority::PAAuto));

        if(!finishPlanPlace){
            pushEvent(WorkerEvent("pickSenorFunction", QVariantList(), WorkerEventPriority::PAAuto));
            pushEvent(WorkerEvent("pickLensFunction", QVariantList(), WorkerEventPriority::PAAuto));
        }
    }
    else if (type == AAResult::NgLens) {
        // to do for hardware fixing
        //pushEvent(WorkerEvent("placeNgLensFunction", QVariantList(), WorkerEventPriority::PAAuto));

        pushEvent(WorkerEvent("pickLensFunction", QVariantList(), WorkerEventPriority::PAAuto));
    }
    else if (type == AAResult::NgSensor) {
        pushEvent(WorkerEvent("placeNgSensorFunction", QVariantList(), WorkerEventPriority::PAAuto));
        pushEvent(WorkerEvent("pickSenorFunction", QVariantList(), WorkerEventPriority::PAAuto));
    }
    else {
        throw SilicolAbort(tr("load and unload material type error! type: %1").arg(type));
    }
}

void PickArm::pickLensFunction()
{
    try
    {
        if (paConfig->currentPickLensUnitRowIndex() * paConfig->currentPickLensRowIndexInside() > lensTrayConfig->rows()
            || paConfig->currentPickLensUnitColumnIndex() * paConfig->currentPickLensColumnIndexInside() > lensTrayConfig->columns())
        {
            throw SilicolAbort(tr("Current Lens Tray Index err ! current index should less than tray setting."
                                  "current row: %1; current column: %2"
                                  "row inside: %3; column inside: %4")
                                   .arg(paConfig->currentPickLensUnitRowIndex())
                                   .arg(paConfig->currentPickLensUnitColumnIndex())
                                   .arg(paConfig->currentPickLensRowIndexInside())
                                   .arg(paConfig->currentPickLensColumnIndexInside()));
        }

        if(!pickeLens(paConfig->currentPickLensRowIndexInside(), paConfig->currentPickLensColumnIndexInside()
                                , paConfig->currentPickLensUnitRowIndex(), paConfig->currentPickLensUnitColumnIndex()))
        {
            return;
        }

        lensTrayMoveNext();
    }
    catch (SilicoolException &se)
    {
        if(UIOperation::getIns()->getUIResponse("Lens tray", tr("Lens tray is null or index err! would you like to exchange tray?/r/ncurrent "
                                                                "index: 1%").arg(se.what()), MsgBoxIcon::Error, YesNoBtns) != YesBtn)
        {
            throw SilicolAbort(tr("Senser tray is null! place exchange!"));
        }

        paConfig->setCurrentPickLensUnitColumnIndex(1);

        QVariantList arg;
        arg << "pickLensFunction";

        pushEvent(WorkerEvent("exchangeTray", arg, WorkerEventPriority::ExchangeTray));
    }
}

void PickArm::pickSenorFunction()
{
    try
    {
        if (paConfig->currentPickSenserUnitRowIndex() * paConfig->currentPickSenserRowIndexInside()  > sensorTrayConfig->rows()
            || paConfig->currentPickSenserUnitColumnIndex() * paConfig->currentPickSenserColumnIndexInside()> sensorTrayConfig->columns())
        {
            throw SilicolAbort(tr("Current Sensor Tray Index err ! current index should less than tray setting."
                                  "current row: %1; current column: %2"
                                  "row inside: %3; column inside: %4")
                                   .arg(paConfig->currentPickSenserUnitRowIndex())
                                   .arg(paConfig->currentPickSenserUnitColumnIndex())
                                   .arg(paConfig->currentPickSenserRowIndexInside())
                                   .arg(paConfig->currentPickSenserColumnIndexInside()));
        }

        if(!pickSensor(paConfig->currentPickSenserRowIndexInside(), paConfig->currentPickSenserColumnIndexInside()
                                 , paConfig->currentPickSenserUnitRowIndex(), paConfig->currentPickSenserUnitColumnIndex()))
        {
            return;
        }

        sensorTrayMoveNext();

    }
    catch (SilicoolException &se)
    {
        paConfig->setCurrentPickSenserUnitColumnIndex(1);

        QVariantList args;
        args << "pickSenorFunction";

        pushEvent(WorkerEvent("exchangeTray", args, WorkerEventPriority::ExchangeTray));
    }
}

void PickArm::placeLensToLUT()
{
    try
    {
        if(!paConfig->isDryRun())
        {
            if (!lensPickerVacuum->get())
            {
                lensPickerVacuum->set(false);
                // to do debug
                return;
                throw SilicolAbort(tr("Picker1 missed lens!"));
            }
        }
        moveUpAllZ();

        pickerArmY->moveTo(SAxisPosName::PlaceLensPos);

//        double curX = SUTxyz->xAxis()->getFeedbackPos();
//        double curY = SUTxyz->yAxis()->getFeedbackPos();
//        double curZ = SUTxyz->zAxis()->getFeedbackPos();

//        auto targetPos = SUTxyz->getPos<XYZModulePos>(XYZPosName::LoadMaterialPos);

//        if(targetPos->xPos() - SUTxyz->xAxis()->config()->)
//        {

//        }
//        else
        {
           SUTxyz->moveTo(XYZPosName::LoadMaterialPos);
        }

        PickerZ2->moveTo(SAxisPosName::PlaceLensPos);

        if(!paConfig->isDryRun())
        {
            lutVacuum->set(true);
            lensPickerVacuum->set(false);
        }
        if (paConfig->delayOfterCloseVacuum() > 0)
        {
            QThread::msleep(paConfig->delayOfterCloseVacuum());
        }
        moveUpAllZ();
    }
    catch (SilicoolException &se)
    {
        throw se;
    }
}

void PickArm::placeSensorToSUT()
{
    try
    {
        if(!paConfig->isDryRun())
        {
            if (!sensorPickerVacuum->get())
            {
                sensorPickerVacuum->set(false);
                // to do debug
                return;
                throw SilicolAbort(tr("Picker1 missed sensor!"));
            }
        }
        moveUpAllZ();

        pickerArmY->moveTo(SAxisPosName::PlaceSensorPos);
        SUTxyz->moveTo(XYZPosName::LoadMaterialPos);
        PickerZ1->moveTo(SAxisPosName::PlaceSenserStandbyPos);
        if(!paConfig->isDryRun())
        {
            if(sutCylinder->get())
            {
                PlaceSensorToSUTErr err;

                auto option = ErrorHandler::getErrorHandlingOption(err);
                if(option == ErrorHandlingOption::Extend_)
                {
                    sutCylinder->set(false);
                }
                else {
                    throw SilicolAbort(tr("SUT has sensor pleace check!"));
                }

            }
        }
        SUTxyz->moveToPos(XYZPosName::LoadSensorPos, XYZModulePos::Z_Y_X);
        PickerZ1->moveTo(SAxisPosName:: PlaceSensorPos);

        if(!paConfig->isDryRun())
        {
            sutCylinder->set(true);
            sutVacuum->set(true);
            sensorPickerVacuum->set(false);
        }
        if (paConfig->delayOfterCloseVacuum() > 0)
        {
            QThread::msleep(paConfig->delayOfterCloseVacuum());
        }
        moveUpAllZ();

        SUTxyz->moveTo(XYZPosName::StandbyPos);
    }
    catch (SilicoolException &se)
    {
        throw se;
    }
}

void PickArm::pickOkProduct()
{
    try
    {
        if(!paConfig->isDryRun())
        {
            if (lensPickerVacuum->get())
            {
                NuzzleHasMaterial err;
                auto option = ErrorHandler::getErrorHandlingOption(err);

                if(option == ErrorHandlingOption::Continual_)
                {

                }
                else {
                    throw SilicolAbort(tr("picker2 nuzzle has material, can not pick OK product!"));
                }
            }
        }
        moveUpAllZ();
        SUTxyz->moveTo(XYZPosName::UnloadOkProductPos);

        pickerArmY->moveTo(SAxisPosName::PickOKProductPos);
        PickerZ2->moveTo(SAxisPosName::PickOKProductPos);

        if(!paConfig->isDryRun())
        {
            sutCylinder->set(false);

            sutVacuum->set(false);
            lensPickerVacuum->set(true);
        }
        if (paConfig->delayOfterOpenVacuum() > 0)
        {
            QThread::msleep(paConfig->delayOfterOpenVacuum());
        }

        SUTxyz->zAxis()->relMove(-2, true);
        SUTxyz->xAxis()->relMove(6, true);
        moveUpAllZ();
    }
    catch (SilicoolException &se)
    {
        moveUpAllZ();
        throw se;
    }
}

void PickArm::pickNgLens()
{
    try
    {
        if(!paConfig->isDryRun())
        {
            if (lensPickerVacuum->get())
            {
                NuzzleHasMaterial err;
                auto option = ErrorHandler::getErrorHandlingOption(err);

                if(option == ErrorHandlingOption::Continual_)
                {

                }
                else {
                    throw SilicolAbort(tr("picker2 nuzzle has material, can not pick NG lens "));
                }
            }
        }
        moveUpAllZ();

        SUTxyz->moveTo(XYZPosName::LoadMaterialPos);

        pickerArmY->moveTo(SAxisPosName::PickNgLensPos);
        PickerZ2->moveTo(SAxisPosName::PickNgLensPos);

        if(!paConfig->isDryRun())
        {
            lensPickerVacuum->set(true);
        }
        if (paConfig->delayOfterOpenVacuum() > 0)
        {
            QThread::msleep(paConfig->delayOfterOpenVacuum());
        }

        moveUpAllZ();
    }
    catch (SilicoolException &se)
    {
        moveUpAllZ();
        throw se;
    }
}

void PickArm::pickNgSensor(bool isNGProduct)
{
    try
    {
        if(!paConfig->isDryRun())
        {
            if (ngSensorPickerVacuum->get())
            {
                NuzzleHasMaterial err;
                auto option = ErrorHandler::getErrorHandlingOption(err);

                if(option == ErrorHandlingOption::Continual_)
                {

                }
                else {
                    throw SilicolAbort(tr("picker3 nuzzle has material, can not pick NG sensor "));
                }
            }
        }

        moveUpAllZ();
        SUTxyz->moveTo(XYZPosName::UnloadNgSenserPos);
        pickerArmY->moveTo(SAxisPosName::PickNgSensorPos);

        ngSensorPickerCylinder->set(true);

        if(paConfig->rejectUseSoftLand())
        {
            SUTxyz->zAxis()->softLandPosDown(SLPos::PickRejectSensor);
            SUTxyz->zAxis()->relMove(-0.001, true);
        }
        else
        {
            auto config = SUTxyz->zAxis()->config();

            auto vcmConfig = qobject_cast<VCMConfig *>(config);
            if (vcmConfig == nullptr)
            {
                throw SilicolAbort(
                    QString("Can not cast config to VCMConfig! Config type: %1").arg(config->metaObject()->className()));
            }
            auto softLandPos = vcmConfig->getPos(SLPos::PickRejectSensor);
            double pos = isNGProduct ? softLandPos->targetPos() - 3.56 : softLandPos->targetPos();

            SUTxyz->zAxis()->absMove(pos, true);
        }

        if(!paConfig->isDryRun())
        {
            sutCylinder->set(false);
            sutVacuum->set(false);
            ngSensorPickerVacuum->set(true);
            if (!ngSensorPickerVacuum->waitUntilTimeout(true))
            {
                ngSensorPickerVacuum->set(false);
                throw SilicolAbort(tr("Picker3 get NG sensor overtime !"));
            }
        }
        if (paConfig->delayOfterOpenVacuum() > 0)
        {
            QThread::msleep(paConfig->delayOfterOpenVacuum());
        }

        SUTxyz->zAxis()->relMove(-2, true);
        SUTxyz->xAxis()->relMove(6, true);
        moveUpAllZ();
    }
    catch (SilicoolException &se)
    {
        ngSensorPickerVacuum->set(false);
        moveUpAllZ();
        throw se;
    }
}

void PickArm::moveTrayLoaderToExchangePos()
{
    moveUpAllZ();
    trayLoaderX->moveTo(SAxisPosName::StandbyPos);
}

void PickArm::placeMaterial(QString type)
{
    QVariantList arg;
    arg<< type;

    if(type == AAResult::FirstLoad)
    {
        finishProduct = false;
        finishPlanPlace = false;

        paConfig->selfAddFinishedCount();
        if(paConfig->finishedCount() >= paConfig->planCount())
        {
            finishPlanPlace = true;
        }
        // to do first load if need to show box

        pushEvent(WorkerEvent("placeLensToLUT", QVariantList(), WorkerEventPriority::PAAuto));
        pushEvent(WorkerEvent("placeSensorToSUT", QVariantList(), WorkerEventPriority::PAAuto));
        pushEvent(WorkerEvent("postAAHeadRun", arg, WorkerEventPriority::PAAuto));
    }
    else if (type == AAResult::GoodProduct) { 
        if(finishPlanPlace)
        {
            finishProduct = true;
            pushEvent(WorkerEvent("pickOkProduct", QVariantList(), WorkerEventPriority::PAAuto));
            pushEvent(WorkerEvent("postAAHeadRun", arg, WorkerEventPriority::PAAuto));
        }
        else {
            paConfig->selfAddFinishedCount();
            if(paConfig->finishedCount() >= paConfig->planCount())
            {
                finishPlanPlace = true;
            }

            pushEvent(WorkerEvent("placeLensToLUT", QVariantList(), WorkerEventPriority::PAAuto));
            pushEvent(WorkerEvent("pickOkProduct", QVariantList(), WorkerEventPriority::PAAuto));
            pushEvent(WorkerEvent("placeSensorToSUT", QVariantList(), WorkerEventPriority::PAAuto));
            pushEvent(WorkerEvent("postAAHeadRun", arg, WorkerEventPriority::PAAuto));
        }
    }
    else if (type == AAResult::NgProduct) {
        if(finishPlanPlace)
        {
            pushEvent(WorkerEvent("pickNGProductFunction", QVariantList(), WorkerEventPriority::PAAuto));
            pushEvent(WorkerEvent("postAAHeadRun", arg, WorkerEventPriority::PAAuto));
        }
        else {
            paConfig->selfAddFinishedCount();
            if(paConfig->finishedCount() >= paConfig->planCount())
            {
                finishPlanPlace = true;
            }

            pushEvent(WorkerEvent("placeLensToLUT", QVariantList(), WorkerEventPriority::PAAuto));
            pushEvent(WorkerEvent("pickNGProductFunction", QVariantList(), WorkerEventPriority::PAAuto));
            pushEvent(WorkerEvent("placeSensorToSUT", QVariantList(), WorkerEventPriority::PAAuto));
            pushEvent(WorkerEvent("postAAHeadRun", arg, WorkerEventPriority::PAAuto));
        }
    }
    else if (type == AAResult::NgLens) {
        pushEvent(WorkerEvent("placeLensToLUT", QVariantList(), WorkerEventPriority::PAAuto));
        // to do for haedware fixing
        //pushEvent(WorkerEvent("pickNgLens", QVariantList(), WorkerEventPriority::PAAuto));
        pushEvent(WorkerEvent("postAAHeadRun", arg, WorkerEventPriority::PAAuto));
    }
    else if (type == AAResult::NgSensor) {
        pushEvent(WorkerEvent("pickNgSensor", QVariantList(), WorkerEventPriority::PAAuto));
        pushEvent(WorkerEvent("placeSensorToSUT", QVariantList(), WorkerEventPriority::PAAuto));
        pushEvent(WorkerEvent("postAAHeadRun", arg, 2000));
    }
    else {
        throw SilicolAbort(tr("place material to sut type error! type: %1").arg(type));
    }
}

void PickArm::postAAHeadRun(QString type)
{
    QVariantList arg;
    arg<< type;
    moveUpAllZ();
    auto pos = PickerXYmodule->getPos<XYModulePos>(XYPosName::PAWaitForSutPos);
    PickerXYmodule->yAxis()->absMove(pos->yPos());
    if(type == "")
    {
        throw SilicolAbort(tr("currentResultType error! type: %1").arg(type));
    }

    pushEvent(WorkerEvent("loadMaterialFunction", arg, WorkerEventPriority::PAAuto));

    if(!finishPlanPlace)
    {
        postEvent(WorkerName::AaHead, WorkerEvent("AAHeadRun", arg));
    }
    else if (finishPlanPlace && (paConfig->finishedCount() == paConfig->planCount()) && !finishProduct) {
        // to aa laset one
        postEvent(WorkerName::AaHead, WorkerEvent("AAHeadRun", arg));
    }
}

void PickArm::exchangeTray(QString trayType)
{
    try {
        moveUpAllZ();
        PickerXYmodule->moveTo(XYPosName::ExchangePanelPos);

        QString str = "Are you sure exchang tray finish?";

        if(!UIOperation::getIns()->yesNoConfirm(str))
        {
            throw SilicolAbort("new tray not ready!");
        }

        moveUpAllZ();
        PickerXYmodule->moveTo(XYPosName::StandbyPos);

    } catch (SilicolAbort &se) {
        QVariantList arg;
        arg<< trayType;
        pushEvent(WorkerEvent("exchangeTray", arg, 3000));
    }



}

void PickArm::placeOkProductFunction()
{
    try
    {
        if (paConfig->currentPlaceProductRowIndexInside() >= paConfig->currentPickSenserRowIndexInside()
            || paConfig->currentPlaceProductColumnIndexInside() > paConfig->currentPickSenserColumnIndexInside()
            || paConfig->currentPlaceProductUintRowIndex() > paConfig->currentPickSenserUnitRowIndex()
            || paConfig->currentPlaceProductUintColumnIndex() > paConfig->currentPickSenserUnitColumnIndex())
        {
            throw SilicolAbort(tr("Current OK product Index err ! current index should less than pick senser indext. please reset"
                                  "current row: %1; current column: %2"
                                  "row inside: %3; column insiide: %4")
                                   .arg(paConfig->currentPlaceProductUintRowIndex())
                                   .arg(paConfig->currentPlaceProductUintColumnIndex())
                                   .arg(paConfig->currentPlaceProductRowIndexInside())
                                   .arg( paConfig->currentPlaceProductColumnIndexInside()));
        }

        placeOkProduct(paConfig->currentPlaceProductRowIndexInside(), paConfig->currentPlaceProductColumnIndexInside()
                       , paConfig->currentPlaceProductUintRowIndex(), paConfig->currentPlaceProductUintColumnIndex());

        placeProducttMoveNext();

    }
    catch (SilicoolException &se)
    {
        throw SilicolAbort(tr("place ok product failed!"));
    }
}

void PickArm::pickNGProductFunction()
{
    pickNgSensor(true);
}

void PickArm::placeNGProductFunction()
{
    checkIsInit();
    placeNgSensorFunction();
}

void PickArm::placeNgLensFunction()
{
    try
    {
        if (paConfig->currentPlaceNgLensRowIndexInside() >= paConfig->currentPickLensRowIndexInside()
                || paConfig->currentPlaceNgLensColumnIndexInside() > paConfig->currentPickLensColumnIndexInside()
                || paConfig->currentPlaceNgLensUintRowIndex() > paConfig->currentPickLensUnitRowIndex()
                || paConfig->currentPlaceNgLensUintColumnIndex() > paConfig->currentPickLensUnitColumnIndex())
        {
            throw SilicolAbort(tr("Current place ng Lens Index err ! current index should less than pick lens index, are you sure reset?"
                                  "current row: %1; current column: %2"
                                  "row inside: %3; column inside: %4")
                                   .arg(paConfig->currentPlaceNgLensUintRowIndex())
                                   .arg(paConfig->currentPlaceNgLensUintColumnIndex())
                                   .arg(paConfig->currentPlaceNgLensRowIndexInside())
                                   .arg(paConfig->currentPlaceNgLensColumnIndexInside()));
        }

        placeNgLens(paConfig->currentPlaceNgLensRowIndexInside(), paConfig->currentPlaceNgLensColumnIndexInside(),
                    paConfig->currentPlaceNgLensUintRowIndex(), paConfig->currentPlaceNgLensUintColumnIndex());

        paConfig->selfAddPlaceNgLensRowIndexInside();

        if(paConfig->currentPlaceNgLensRowIndexInside() > lensTrayConfig->rowsInsideUnit())
        {
            paConfig->setCurrentPlaceNgLensRowIndexInside(1);
            paConfig->selfAddPlaceNgLensColumnIndexInside();
        }

        if(paConfig->currentPlaceNgLensColumnIndexInside() > lensTrayConfig->columnsInsideUnit())
        {
            paConfig->setCurrentPlaceNgLensColumnIndexInside(1);
            paConfig->selfAddPlaceNgLensUintRowIndex();
        }

        if(paConfig->currentPlaceNgLensUintRowIndex() > lensTrayConfig->unitRows())
        {
            paConfig->setCurrentPlaceNgLensUintRowIndex(1);
            paConfig->selfAddPlaceNgLensUintColumnIndex();
        }

        if(paConfig->currentPlaceNgLensUintColumnIndex() > lensTrayConfig->unitColumns())
        {
            paConfig->setCurrentPlaceNgLensUintColumnIndex(1);
        }
    }
    catch (SilicoolException &se)
    {

    }
}

void PickArm::placeNgSensorFunction()
{
    try
    {
        if (paConfig->currentRejectTrayRowIndexInside() * paConfig->currentRejectTrayUnitRowIndex() > rejectTrayConfig->rows()
            || paConfig->currentRejectTrayColumnIndexInside() * paConfig->currentRejectTrayUnitColumnIndex() > rejectTrayConfig->columns())
        {
            throw SilicolAbort(tr("Current reject sensor tray is full !  please remove!"
                                  "current row: %1; current column: %2"
                                  "row inside: %3; column inside: %4")
                                   .arg(paConfig->currentRejectTrayUnitRowIndex())
                                   .arg(paConfig->currentRejectTrayUnitColumnIndex())
                                   .arg(paConfig->currentRejectTrayRowIndexInside())
                                   .arg(paConfig->currentRejectTrayColumnIndexInside()));
        }

        placeNgSensor(paConfig->currentRejectTrayRowIndexInside(), paConfig->currentRejectTrayColumnIndexInside(),
                      paConfig->currentRejectTrayUnitRowIndex(), paConfig->currentRejectTrayUnitColumnIndex());

        paConfig->selfAddRejectTrayRowIndexInside();

        if(paConfig->currentRejectTrayRowIndexInside() > rejectTrayConfig->rowsInsideUnit())
        {
            paConfig->setCurrentRejectTrayRowIndexInside(1);
            paConfig->selfAddRejectTrayColumnIndexInside();
        }

        if(paConfig->currentRejectTrayColumnIndexInside() > rejectTrayConfig->columnsInsideUnit())
        {
            paConfig->setCurrentRejectTrayColumnIndexInside(1);
            paConfig->selfAddRejectTrayUnitRowIndex();
        }

        if(paConfig->currentRejectTrayUnitRowIndex() > rejectTrayConfig->unitRows())
        {
            paConfig->setCurrentRejectTrayUnitRowIndex(1);
            paConfig->selfAddRejectTrayUnitColumnIndex();
        }
    }
    catch (SilicoolException &se)
    {
        paConfig->setCurrentRejectTrayUnitColumnIndex(1);

        pushEvent(WorkerEvent("placeNgSensorFunction", QVariantList(), 3000));
    }
}

void PickArm::calibratePickerToDownLookCameraOffset(bool isSensorPicker)
{
    checkIsInit();

    if (!UIOperation::getIns()->okCancelConfirm(tr("Please place calibration glass at the view of lens tray camera!")))
    {
        return;
    }
    if (paConfig->corCaliStepCount() < 3)
    {
        throw SilicolAbort(tr("COR calibration step count must greater than 2!"));
    }

    PrOffset caliGlassOffset = getCaliGlassPrOffset();
    double prPosX = PickerXYmodule->xAxis()->getFeedbackPos(-1);
    double prPosY = PickerXYmodule->yAxis()->getFeedbackPos(-1);
    double pickerToCameraOffsetX = isSensorPicker ? paConfig->picker1ToCameraOffsetX() : paConfig->picker2ToCameraOffsetX();
    double pickerToCameraOffsetY = isSensorPicker ? paConfig->picker1ToCameraOffsetY() : paConfig->picker2ToCameraOffsetY();
    QPointF pickShift = QPointF(-caliGlassOffset.X, -caliGlassOffset.Y) + QPointF(pickerToCameraOffsetX, pickerToCameraOffsetY);

    PickerT1->moveTo(SAxisPosName::StandbyPos);
    PickerT2->moveTo(SAxisPosName::StandbyPos);

    QVector<QPointF> offsets;
    for (int i = 0; i < paConfig->corCaliStepCount(); i++)
    {
        PrOffset offset;
        if(i == 0)
        {
            offset = caliGlassOffset;
        }
        else
        {
             offset = getCaliGlassPrOffset();
        }
        offsets.append(QPointF(offset.X, offset.Y));
        if (i != paConfig->corCaliStepCount() - 1)
        {
            PickerXYmodule->relMove(pickShift);
            pickCalibGlass(isSensorPicker);

            if (isSensorPicker)
            {
                PickerT1->axis()->relMove(paConfig->corCaliStepSize());
            }
            else
            {
                PickerT2->axis()->relMove(paConfig->corCaliStepSize());
            }
            QThread::msleep(200);
            placeCalibGlass(isSensorPicker);
            PickerXYmodule->moveTo(QPointF(prPosX, prPosY));
            QThread::msleep(400);
        }
    }

    if (isSensorPicker)
    {
        PickerT1->moveTo(SAxisPosName::StandbyPos);
    }
    else
    {
        PickerT2->moveTo(SAxisPosName::StandbyPos);
    }

    double radius;
    QPointF nozzelCenter;
    fitCircle(offsets, nozzelCenter, radius);
    QPointF offsetShift = nozzelCenter - QPointF(caliGlassOffset.X, caliGlassOffset.Y);

    double newPicker1ToCameraOffsetX = pickerToCameraOffsetX + offsetShift.x();
    double newPicker1ToCameraOffsetY = pickerToCameraOffsetY + offsetShift.y();

    QString bhToLTCameraMsg
        = tr("Calibrate picker to sut Down look camera offset successful!\r\nOffset x: %1, Offset y: "
             "%2\r\nNew picker to sut Down look camera offset x: %3, y: %4\r\nWould you like to use the new offset ?")
              .arg(offsetShift.x())
              .arg(offsetShift.y())
              .arg(newPicker1ToCameraOffsetX)
              .arg(newPicker1ToCameraOffsetY);

    if (UIOperation::getIns()->yesNoConfirm(bhToLTCameraMsg))
    {
        if (isSensorPicker)
        {
            paConfig->setPicker1ToCameraOffsetX(newPicker1ToCameraOffsetX);
            paConfig->setPicker1ToCameraOffsetY(newPicker1ToCameraOffsetY);
        }
        else
        {
            paConfig->setPicker2ToCameraOffsetX(newPicker1ToCameraOffsetX);
            paConfig->setPicker2ToCameraOffsetY(newPicker1ToCameraOffsetY);
        }
    }
}

void PickArm::moveToPickCalibrateGlass(bool isSensorPicker)
{
    checkIsInit();

    moveUpAllZ();

    if (isSensorPicker)
    {
        PickerXYmodule->moveTo(XYPosName::Picker1GetcalibGrassPos);
        pickCalibGlass(isSensorPicker);
    }
    else
    {
        PickerXYmodule->moveTo(XYPosName::Picker2GetcalibGrassPos);
        pickCalibGlass(isSensorPicker);
    }
}

void PickArm::moveToPickerPrPos(bool isSensorPicker)
{
    checkIsInit();
    moveToPADownlookPrPos(isSensorPicker);
}

void PickArm::moveToPickLens(int rowIndexInside, int columnIndexInside, int unitRowIndex, int unitColumnIndex)
{
    checkIsInit();
    if (rowIndexInside > lensTrayConfig->rows() || columnIndexInside > lensTrayConfig->columns())
    {
        throw SilicolAbort(tr("Current Lens Tray Index err ! current index should less than tray setting."
                              "current row: %1; current column: %2"
                              "tray row: %3; tray column: %4")
                               .arg(rowIndexInside)
                               .arg(columnIndexInside)
                               .arg(lensTrayConfig->rows())
                               .arg(lensTrayConfig->columns()));
    }
    pickeLens(rowIndexInside, columnIndexInside, unitRowIndex, unitColumnIndex);
}

void PickArm::moveToPickSensor(int rowIndexInside, int columnIndexInside, int unitRowIndex, int unitColumnIndex)
{
    checkIsInit();
    if (rowIndexInside > sensorTrayConfig->rows() || columnIndexInside > sensorTrayConfig->columns())
    {
        throw SilicolAbort(tr("Current sensor Tray Index err ! current index should less than tray setting."
                              "current row: %1; current column: %2"
                              "tray row: %3; tray column: %4")
                               .arg(rowIndexInside)
                               .arg(columnIndexInside)
                               .arg(sensorTrayConfig->rows())
                               .arg(sensorTrayConfig->columns()));
    }
    pickSensor(rowIndexInside, columnIndexInside, unitRowIndex, unitColumnIndex);
}

void PickArm::moveToPlaceLensToLut()
{
    checkIsInit();

    placeLensToLUT();
}

void PickArm::moveToPlaceSensorToSut()
{
    checkIsInit();
    placeSensorToSUT();
}

void PickArm::moveToLensPrPos(int rowIndexInside, int columnIndexInside, int unitRowIndex, int unitColumnIndex)
{
    if (rowIndexInside * unitRowIndex > lensTrayConfig->rows()
        || columnIndexInside * unitColumnIndex > lensTrayConfig->columns())
    {
        throw SilicolAbort(tr("Current Lens Tray Pr Index err ! current index should less than tray setting."
                              "current row: %1; current column: %2"
                              "row inside: %3; column inside: %4")
                               .arg(paConfig->currentPickLensUnitRowIndex())
                               .arg(paConfig->currentPickLensUnitColumnIndex())
                               .arg(paConfig->currentPickLensRowIndexInside())
                               .arg(paConfig->currentPickLensColumnIndexInside()));
    }

    auto lensPrPos = PickerXYmodule->getPos<XYModulePos>(XYPosName::LensTrayDownLookPos);
    double xPos = lensPrPos->xPos() - (unitColumnIndex -1) * lensTrayConfig->unitColumnDelta() - (columnIndexInside - 1) * lensTrayConfig->columnDelta();
    double yPos = lensPrPos->yPos() + (unitRowIndex -1) * lensTrayConfig->unitRowDelta() + ( rowIndexInside- 1) * lensTrayConfig->rowDelta();
    PickerXYmodule->moveTo(QPointF(xPos, yPos));
}

void PickArm::moveToSensorPrPos(int rowIndexInside, int columnIndexInside, int unitRowIndex, int unitColumnIndex)
{
    if (rowIndexInside * unitRowIndex > sensorTrayConfig->rows()
        || columnIndexInside * unitColumnIndex > sensorTrayConfig->columns())
    {
        throw SilicolAbort(tr("Current Sensor Tray Pr Index err ! current index should less than tray setting."
                              "current row: %1; current column: %2"
                              "row inside: %3; column inside: %4")
                               .arg(paConfig->currentPickLensUnitRowIndex())
                               .arg(paConfig->currentPickLensUnitColumnIndex())
                               .arg(paConfig->currentPickLensRowIndexInside())
                               .arg(paConfig->currentPickLensColumnIndexInside()));
    }

    auto lensPrPos = PickerXYmodule->getPos<XYModulePos>(XYPosName::SensorTrayDownLookPos);
    double xPos = lensPrPos->xPos() - (unitColumnIndex -1) * sensorTrayConfig->unitColumnDelta() - (columnIndexInside - 1) * sensorTrayConfig->columnDelta();
    double yPos = lensPrPos->yPos() + (unitRowIndex -1) * sensorTrayConfig->unitRowDelta() + ( rowIndexInside- 1) * sensorTrayConfig->rowDelta();
    PickerXYmodule->moveTo(QPointF(xPos, yPos));
}

void PickArm::moveToRejectSensorPos(int rowIndexInside, int columnIndexInside, int unitRowIndex, int unitColumnIndex)
{
    if ( rowIndexInside* unitRowIndex > rejectTrayConfig->rows()
        || columnIndexInside* unitColumnIndex > rejectTrayConfig->columns())
    {
        throw SilicolAbort(tr("Current reject sensor tray Pr index is full !  please remove!"
                              "current row: %1; current column: %2"
                              "row inside: %3; column inside: %4")
                               .arg(paConfig->currentRejectTrayUnitRowIndex())
                               .arg(paConfig->currentRejectTrayUnitColumnIndex())
                               .arg(paConfig->currentRejectTrayRowIndexInside())
                               .arg(paConfig->currentRejectTrayColumnIndexInside()));
    }

    auto pos = PickerXYmodule->getPos<XYModulePos>(XYPosName::PlaceNgSenserPos);
    PickerXYmodule->moveTo(QPointF(pos->xPos() - (unitColumnIndex - 1)* rejectTrayConfig->unitColumnDelta() - (columnIndexInside - 1) * rejectTrayConfig->columnDelta(),
                                   pos->yPos() + (unitRowIndex - 1)* rejectTrayConfig->unitRowDelta() + (rowIndexInside - 1)* rejectTrayConfig->rowDelta()));
}

void PickArm::moveToGetRejectLens()
{
    checkIsInit();
    pickNgLens();
}

void PickArm::moveToGetRejectSensor()
{
    checkIsInit();
    pickNgSensor(false);
}

void PickArm::moveToGetOKProduct()
{
    checkIsInit();
    pickOkProduct();
}

void PickArm::moveToPlaceRejectLens(int rowIndexInside, int columnIndexInside, int unitRowIndex, int unitColumnIndex)
{
    checkIsInit();
    if (rowIndexInside *unitRowIndex  > lensTrayConfig->rows() || columnIndexInside * unitColumnIndex > lensTrayConfig->columns())
    {
        throw SilicolAbort(tr("Current place reject Lens Index err ! current index should less than tray setting."
                              "current row: %1; current column: %2"
                              "tray row: %3; tray column: %4")
                               .arg(rowIndexInside)
                               .arg(columnIndexInside)
                               .arg(lensTrayConfig->rows())
                               .arg(lensTrayConfig->columns()));
    }

    placeNgLens(rowIndexInside, columnIndexInside, unitRowIndex, unitColumnIndex);
}

void PickArm::moveToPlaceRejectSensor(int rowIndexInside, int columnIndexInside, int unitRowIndex, int unitColumnIndex)
{
    checkIsInit();
    if (rowIndexInside *unitRowIndex > rejectTrayConfig->rows() || columnIndexInside * unitColumnIndex > rejectTrayConfig->columns())
    {
        throw SilicolAbort(tr("Current place reject sensor Index err ! current index should less than tray setting."
                              "current row: %1; current column: %2"
                              "tray row: %3; tray column: %4")
                               .arg(rowIndexInside)
                               .arg(columnIndexInside)
                               .arg(rejectTrayConfig->rows())
                               .arg(rejectTrayConfig->columns()));
    }

    placeNgSensor(rowIndexInside, columnIndexInside, unitRowIndex, unitColumnIndex);
}

void PickArm::moveToPlaceOKProduct(int rowIndexInside, int columnIndexInside, int unitRowIndex, int unitColumnIndex)
{
    checkIsInit();
    if (rowIndexInside > sensorTrayConfig->rows() || columnIndexInside > sensorTrayConfig->columns())
    {
        throw SilicolAbort(tr("Current place ok product Index err ! current index should less than tray setting."
                              "current row: %1; current column: %2"
                              "tray row: %3; tray column: %4")
                               .arg(rowIndexInside)
                               .arg(columnIndexInside)
                               .arg(sensorTrayConfig->rows())
                               .arg(sensorTrayConfig->columns()));
    }

    placeOkProduct(rowIndexInside, columnIndexInside, unitRowIndex, unitColumnIndex);
}


void PickArm::pickGlassWithOffset(bool isSensorPicker)
{
    checkIsInit();

    moveUpAllZ();

    PrOffset prOffset = getCaliGlassPrOffset();

    if (isSensorPicker)
    {
        PickerXYmodule->relMove(QPointF(- prOffset.X + paConfig->picker1ToCameraOffsetX(),
                                       - prOffset.Y + paConfig->picker1ToCameraOffsetY()));
        pickCalibGlass(isSensorPicker);
    }
    else
    {
        PickerXYmodule->relMove(QPointF(- prOffset.X + paConfig->picker2ToCameraOffsetX(),
                                       - prOffset.Y + paConfig->picker2ToCameraOffsetY()));
        pickCalibGlass(isSensorPicker);
    }
}

